#include "IMUTask.h"

#include "main.h"
#include "FreeRTOS.h"
#include "task.h"
#include "MPU6050.h"
#include "control.h"	
#include "tim.h"
void IMU_Task(void* pvParameters)
{
	
	static portTickType PreviousWakeTime; 
	PreviousWakeTime = xTaskGetTickCount();
	
	taskENTER_CRITICAL(); //进入临界区
//	HAL_TIM_Encoder_Start(&htim1,TIM_CHANNEL_ALL);
//	HAL_TIM_Encoder_Start(&htim4,TIM_CHANNEL_ALL);
//	HAL_GPIO_WritePin(STBY_GPIO_Port,STBY_Pin,GPIO_PIN_SET);
//	HAL_GPIO_WritePin(AIN1_GPIO_Port,AIN1_Pin,GPIO_PIN_SET);
//	HAL_GPIO_WritePin(AIN2_GPIO_Port,AIN2_Pin,GPIO_PIN_RESET);
//	HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_1);
//	HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_4);
	IIC_Init();                     //=====模拟IIC初始匿
  MPU6050_initialize();           //=====MPU6050初始匿	
  DMP_Init();                     //=====初始化DMP  
	taskEXIT_CRITICAL(); //退出临界区
	while(1)
	{
		Read_DMP();
		vTaskDelayUntil(&PreviousWakeTime,pdMS_TO_TICKS(5));		//绝对延时函数 保证PID时间准确性
	}
}

